#include "robotfinder.hpp"
#include "string.h"

const int Finder::BLUE  = 0;
const int Finder::RED   = 2;
const int Finder::GREEN = 1;
const int Finder::ROWS = 256;
const int Finder::COLS = 256;



Mat bwSlice(Mat image){
    Mat outIm(image.rows,image.cols,CV_8UC1);
    int colsIm = image.cols;
    int cnIm = image.channels();
    for(int i = 0; i < image.rows; i++)
	{
	    for(int j = 0; j < image.cols; j++)
	    {
            outIm.data[i*colsIm + j] =  image.data[i*colsIm*cnIm + j*cnIm+2];
					
		}
    }   
    // Return image
    return outIm;

}


Finder::Finder() { 
    camera_ = StartCamera(ROWS, COLS, 20, 1, true);
    InitGraphics();
	thresholds_ = NULL;
	setThresholdsToDefaults();
    
    currentTex_  = new GfxTexture();    
    currentTex_->CreateRGBA(COLS,ROWS);
    currentTex_->GenerateFrameBuffer();

    tempTex0_ = new GfxTexture();
    tempTex0_->CreateRGBA(COLS,ROWS);
    tempTex0_->GenerateFrameBuffer();
    
    tempTex1_ = new GfxTexture();    
    tempTex1_->CreateRGBA(COLS,ROWS);
    tempTex1_->GenerateFrameBuffer();
    BeginFrame();
    
    
}

Finder::~Finder(){
	if (thresholds_ != NULL){	
		delete thresholds_;
	}
    StopCamera();
    EndFrame();
    ReleaseGraphics();
    for (size_t i = 0; i < binImages_.size(); i++){
        GfxTexture * tex = binImages_.back();
        binImages_.pop_back();
        delete tex;
    }
}

void Finder::filter(Mat &image){
	/*// TODO: Do proper error checking	
	if (threshSize_ < 1){
		throw new ImageProcessingException(
			"Must be at least one filter to filter");
	}

	// Setup for walking through image
	int cnIm  = image.channels();
	int colsIm = image.cols;

	// Setup for preping output
	int cnOut = binImages_[0].channels();
	uchar temp;

	// Walk through image and threshold	
	for(int i = 0; i < image.rows; i++)
	{
	    for(int j = 0; j < image.cols; j++)
	    {
		for (int k = 0; k < threshSize_; k++){
				//cerr << "Got Here!";
				temp = pixelThreshold(
					thresholds_ + k,
					image.data + i*colsIm*cnIm + j*cnIm + RED,
					image.data + i*colsIm*cnIm + j*cnIm + GREEN,
					image.data + i*colsIm*cnIm + j*cnIm + BLUE);

				binImages_[k].data[i*colsIm*cnOut + j*cnOut] = 255 * temp;

					
				
		}
	    }
	}*/
}

void Finder::setRobotColorThresholds(vector<ColorThreshold>& thresholds){
	// Clear binImages so that it can be repopulated 	
	binImages_.clear();

	// Get an image from the camera to use as a model for the threshold 
	// images
    
	// Create an image for each threshold
	for (int i = 0; i < thresholds.size(); i++){
        binImages_.push_back(new GfxTexture());
        binImages_.back()->CreateRGBA(COLS,ROWS);
        binImages_.back()->GenerateFrameBuffer();
	}

	// Clear the thresholds
	if (thresholds_ != NULL){	
		delete thresholds_;
		thresholds_ = NULL;
	}

	// Copy over the new thresholds
	threshSize_ = thresholds.size();
	thresholds_ = new ColorThreshold[threshSize_];
	for (int i = 0; i < threshSize_; i++){
		thresholds_[i] = thresholds[i];
	
	}
	
}


void Finder::setThresholdsToDefaults(){
	
	vector<ColorThreshold> colors;
	double slop = 1.0;
	colors.push_back(ColorThreshold());
	colors[0].redThresh = 211;
	colors[0].greenThresh = 107;	
	colors[0].blueThresh = 110;

	colors[0].redSDev = (int) (24.0 * slop);
	colors[0].greenSDev = (int) (24.0  * slop);
	colors[0].blueSDev = (int) (20.0 * slop);

	strcpy(colors[0].label, "RED");
	
	colors.push_back(ColorThreshold());	
	colors[1].redThresh = 18;
	colors[1].greenThresh = 164;	
	colors[1].blueThresh = 157;

	colors[1].redSDev = (int)(36.0  * slop);
	colors[1].greenSDev = (int)(24.0  * slop);
	colors[1].blueSDev = (int)(29.0 * slop);

	strcpy(colors[1].label, "GREEN");


	colors.push_back(ColorThreshold());	
	colors[2].redThresh = 25;
	colors[2].greenThresh = 64;	
	colors[2].blueThresh = 162;

	colors[2].redSDev = (int) (29.0 * slop);
	colors[2].greenSDev = (int) (20.0 * slop);
	colors[2].blueSDev = (int) (28.0 * slop);

	strcpy(colors[2].label, "BLUE");
	
	setRobotColorThresholds(colors);

}

vector<ColorThreshold> Finder::getRobotColorThresholds(){
	// Retrieves colorthresholds	
	vector<ColorThreshold> threshes;
	for (int i = 0; i < threshSize_; i++){
		threshes.push_back(thresholds_[i]);
	}
	return threshes;
}

vector<Robot> Finder::getRobotPositions(){
	// Create variables	
	vector<Robot> robots;
	vector<vector<Point> > blobs;
	Moments imMoments;
	double x_bar;
	double y_bar;

    const void * tempImage;
    int size_temp;

    int num_tries = 10000;
    
    while(!camera_->BeginReadFrame(0,tempImage,size_temp)){
        if (num_tries == 0){            
            throw new CameraException("Failed to get image!");
        }
        num_tries--;
    }
    {
        currentTex_->SetPixels(tempImage);
    
        camera_->EndReadFrame(0);


    }
	{
        
        
        for (int i = 0; i < threshSize_; i++){
            glBindFramebuffer(GL_FRAMEBUFFER,0);
            DrawThreshRectBGR(currentTex_,thresholds_[i], tempTex0_);
            DrawErodeRect(tempTex0_,tempTex1_);
	    DrawDilateRect(tempTex1_,tempTex0_);
	    DrawDilateRect(tempTex0_,tempTex1_);
   	    DrawDilateRect(tempTex1_,tempTex0_);
            DrawDilateRect(tempTex0_,binImages_[i]);

            

            //Mat temp(ROWS,COLS,CV_8UC3);   
            //glBindFramebuffer(GL_FRAMEBUFFER,binImages_[i]->GetFramebufferId());
            //glReadPixels(0,0,COLS,ROWS,GL_RGB,GL_UNSIGNED_BYTE, 
            //   temp.data);
            //imshow("bin",temp.clone());
            //glBindFramebuffer(GL_FRAMEBUFFER,0);
            //DrawBWLabelRect(tempTex1_,-1.f,-1.f,1.f,1.f,);

            //glBindFramebuffer(GL_FRAMEBUFFER,tempTex0_->GetFramebufferId());
            //glReadPixels(0,0,COLS,ROWS,GL_RGB,GL_UNSIGNED_BYTE, 
            //    temp.data);
            //glBindFramebuffer(GL_FRAMEBUFFER,0);
            //imshow("labeled Image",temp);
            
                    

       }

       

    }
    static Mat binImageRGB(ROWS,COLS,CV_8UC3);
    static Mat binImage(ROWS,COLS,CV_8UC1);
    static int from_to[] = {0,0};
	for (int i = 0; i < binImages_.size(); i++){
        
        glBindFramebuffer(GL_FRAMEBUFFER,binImages_[i]->GetFramebufferId());
        glReadPixels(0,0,COLS,ROWS,GL_RGB,GL_UNSIGNED_BYTE, 
            binImageRGB.data);
        mixChannels(&binImageRGB,1,&binImage,1,from_to,1);

        
		// Find different blobs in the image
		findContours(binImage,blobs,CV_RETR_TREE,CV_CHAIN_APPROX_NONE);

		for (int j = 0; j < blobs.size(); j++){
			findCentroids(blobs[j],x_bar,y_bar);		

			// Create a new robot since we found one
			robots.push_back(Robot());
			// Copy over label from the thresholds 
			strcpy(
				robots[robots.size() - 1].label,
				thresholds_[i].label
			);
			int zero = 0.0;
			robots[robots.size() - 1].position 
				= convertPixelLoc2Polar(   
					&x_bar, &y_bar,&binImage.rows,  
					&binImage.cols
				);
			robots[robots.size() - 1].position.angle_rad = robots[robots.size() - 1].position.angle_rad;
			double distSelf = 3.0;
			// Ignore distances closer than 12 cm becuase they are me
			if (robots[robots.size() - 1].position.radius_cm < distSelf){
				robots.pop_back();
			}
			// If robot positions are closer than 1.0 cm ignore 
			// they are the same item
			else if(robots.size() > 1 &&
				robots[robots.size() - 1].position.radius_cm > 
				robots[robots.size() - 2].position.radius_cm 
				- 1.0 &&
				robots[robots.size() - 1].position.radius_cm < 
				robots[robots.size() - 2].position.radius_cm + 1.0){
				robots.pop_back();
			}
				
		}
        
		
	}
	
	return robots;
}

void Finder::findCentroids(vector<Point> & contours, double& x_bar, double& y_bar){
	x_bar = 0;
	y_bar = 0;	
    
	for(int i = 0; i < contours.size();i++){
		x_bar += contours[i].x;
		y_bar += contours[i].y;				
	}
	x_bar /= contours.size();
	y_bar /= contours.size();

}


Mat Finder::getCurrentImage(){
    // Read from GPU    
    
    glBindFramebuffer(GL_FRAMEBUFFER,currentTex_->GetFramebufferId());
    Mat im(ROWS,COLS,CV_8UC3);
    glReadPixels(0,0,COLS,ROWS,GL_RGB,GL_UNSIGNED_BYTE, 
        im.data);
    glBindFramebuffer(GL_FRAMEBUFFER,0);
    
    Mat imR(ROWS,COLS,CV_8UC3);
    
    // Convert from RGB to BGR
    cv::cvtColor(im, imR, CV_RGB2BGR);
    return imR;
}

vector<Mat> Finder::getBinaryImages(){
    vector<Mat> bins;
    Mat binImageRGB(ROWS,COLS,CV_8UC3);
    Mat binImage(ROWS,COLS,CV_8UC1);
    int from_to[] = {0,0};
        
    for (size_t i = 0; i < binImages_.size(); i++){
        glBindFramebuffer(GL_FRAMEBUFFER,binImages_[i]->GetFramebufferId());
        glReadPixels(0,0,COLS,ROWS,GL_RGB,GL_UNSIGNED_BYTE, 
                binImageRGB.data);
	mixChannels(&binImageRGB,1,&binImage,1,from_to,1);
        bins.push_back(binImage.clone());
        glBindFramebuffer(GL_FRAMEBUFFER,0);
    } 
    return bins;
}


	
